//
// Created by THE13 on 2024/2/6.
//


//二阶系统
#include "animation_sysfun.h"
#include "math.h"

#define PI 3.1415926f

//f是以HZ为单位的系统固有的频率，即每秒的周期数
//z为阻尼系数，它描述了系统最终趋于稳定，当z为0时表示为无阻尼系统，意味着震动永远不会衰减
//r是控制系统的初始响应函数，
// 当r为0时，系统需要花费一定时间才从静止开始加速，
// 当r为正时，系统会对x的运动立即做出反应，
// 当r大于1时，系统对输入的响应会超过输入，即超调，
// 当r为负值时，系统将会预测运动，会有一定的抬手动作
//在为机械连接进行建模时，r值通常取2
void SysInit(SecondOrderDynamics_t *sys,float f,float z,float r,float x0)
{
    //compute constants
    sys->k1 = z/(PI*f);
    sys->k2 = 1/((2*PI*f)*(2*PI*f));
    sys->k3 = r*(2*PI*f);
    //initialize variables
    sys->xp = x0;
    sys->y=x0;
    sys->yd=0;
}


float SysUpdate(SecondOrderDynamics_t *sys,float T,float x,float xd)
{
    //estimate velocity
    if(xd == 0){
        xd = (x-sys->xp)/T;
        sys->xp = x;
    }
//    float k2_stable= fmax(sys->k2 , (T*T/2+T*(sys->k1)/2,T*(sys->k1)));//clamping k2 to guarntee stability mode:1
    float k2_stable= fmax(sys->k2 ,T*T/2+T*((sys->k1)/2));//mode:2 防止福特政治导致频率在较高时出现这种帧时间抖动
    k2_stable = fmax(k2_stable,T*(sys->k1));
    //state variables
    sys->y = sys->y +T*sys->yd;
    sys->yd = sys->yd+T*(x +(sys->k3)*xd-(sys->y)-(sys->k1)*(sys->yd)/k2_stable);
    return sys->y;
}
